/**
 * サーボ位置の初期化
 */
static void servo_ini(int pos)
{
	int i;

	if(pos < POS_NTL || pos >= POS_NUM){
		return;
	}

	for(i = 0; i < SERVO_NUM; i++){
		myservo[i].attach(servo_pin[i]);
		/* 各サーボのニュートラル位置で初期化 */
		myservo[i].write(servo_degs[i][pos]);
	}
}

/**
 * コマンド、PUSH回数と押す/離すの指示から
 * サーボの角度制御を行う
 * サーボ1個のみ制御
 *
 * @param sequence どのシーケンスか
 * @param count Pushカウント(単純なカウンタなので、内部で剰余処理が必要)
 * @param push_ntl 0:離す, 1:押す
 */
static void servo_action(int sequence, unsigned int count, int push_ntl)
{
	Servo act_servo;
	int servo_num;
	int command_id;

	/* 引数チェック */
	if((sequence < 0 || sequence >= SEQ_PTN_NUM)
	|| (push_ntl < 0 || push_ntl > 1)){
		return;
	}

	/* シーケンス、カウントから、実行すべきコマンドIDを取得
	 * 4(COMMAND_DRUM_NUM)で割ると余りが0になるカウント(→ 1つめのコマンド)
	 *                            余りが1になるカウント(→ 2つめのコマンド)
	 */
	command_id = scene[sequence][((count / COMMAND_DRUM_NUM) % SEQUENCE_NUM)];

	/* 現在のコマンド、パターン数から、制御するサーボオブジェクトを取得
	 * 実行すべきコマンドと、COMMAND_DRUM_NUMの何番目か(剰余)でどのボタンを押すか決める
	 */
	servo_num = command_pattern[command_id][count % COMMAND_DRUM_NUM];

	/* ボタンとサーボの対応表から、制御するサーボ番号を取得する */
	act_servo =	myservo[servo_num];

	/* サーボ番号と PUSH/NTL の指示に従ってサーボ角度を取得、書き込み
	 * それぞれのサーボでPUSH/NTL時の角度が違うので、対応表(servo_degsから角度取得)
	 */
	act_servo.write(servo_degs[servo_num][push_ntl]);
}

